#pragma once

#include "PhysicsNode.h"
#include <vector>
#include "../../nclgl/Vector3.h"
#include "Plane_c.h"

using std::vector;

class CollisionData
{
	public:
	Vector3 m_point;
	Vector3 m_normal;
	float m_penetration;
};

enum CollisionVolumeType {
	COLLISION_SPHERE,
	COLLISION_AABB,
	COLLISION_PLANE
};

class CollisionVolume {
public:
	CollisionVolume(){}
	virtual ~CollisionVolume(){
		for(int i = 0; i<children.size(); ++i){
			//delete children[i];
		}
	}

	virtual CollisionVolumeType GetType() = 0;// { return type;}
	Vector3* GetPosition() const {return m_pos;} 

	Vector3* m_pos;
	std::vector<CollisionVolume*> children;
	PhysicsNode* physicsNode;
protected:
	CollisionVolumeType type;
};

class CollisionSphere: public CollisionVolume {
	public:
	CollisionSphere (float r)
	{
		type = COLLISION_SPHERE;
		m_radius = r;
	}
	//Vector3 m_pos;
	float m_radius;
	CollisionVolumeType GetType() { return type; }
};

class CollisionPlane: public CollisionVolume {
public:
	CollisionPlane(const Vector3 &normal , float distance , bool normalise=false){
		type = COLLISION_PLANE;
		if(normalise) {
			float length =sqrt( Vector3 ::Dot(normal, normal));

			this ->normal = normal / length;
			this ->distance = distance / length;
		}else{
			this ->normal = normal;
			this ->distance = distance;
		}
	}

	void SetNormal(const Vector3 &normal) {this ->normal = normal;}
	Vector3 GetNormal () const {return normal;}

	void SetDistance(float dist) {distance = dist;}
	float GetDistance () const {return distance;}

	bool SphereInPlane(const Vector3& position, float radius, CollisionData* collisionData) const{
		float separation = Vector3::Dot(position, normal) + distance;
		if (separation <= -radius){
			return false;
		}
		if (collisionData){
			collisionData->m_penetration = 2*radius - separation/2;
			collisionData->m_normal = -normal;
			collisionData->m_point = position - normal * separation;
		}
		return true;
	}

	CollisionVolumeType GetType() { return type;}
protected:
	Vector3 normal;
	float distance;
};

//Used for BroadPhase
class CollisionAABB: public CollisionVolume {
public:
	CollisionAABB(PhysicsNode* p, float w = 0, float h = 0, float l = 0){
		type = COLLISION_AABB;
		halfdims.x = w/2.0f;
		halfdims.y = h/2.0f;
		halfdims.z = l/2.0f;
		physicsNode = p;
		isStatic = false;
		//m_pos = &(physicsNode->GetPosition());
	}
	Vector3 halfdims;
	CollisionVolumeType GetType()			{ return type; }
	void				SetStatic(bool s)	{ isStatic = s; }
	bool				GetStatic()	const	{ return isStatic; }
protected:
	bool isStatic;
};

class PhysicsSystem	{
public:
	friend class GameClass;

	void		Update(float msec);

	void		BroadPhaseCollisions();
	void		NarrowPhaseCollisions();

	//Statics
	static void Initialise() {
		instance = new PhysicsSystem();
	}

	static void Destroy() {
		delete instance;
		instance = NULL;
	}

	static PhysicsSystem& GetPhysicsSystem() {
		return *instance;
	}

	void	AddNode(PhysicsNode* n);

	void	AddTemp(PhysicsNode* n);

	void	AddStatic(PhysicsNode* n);

	void	RemoveNode(PhysicsNode* n);

	void	RemoveStatic(PhysicsNode* n);

protected:
	PhysicsSystem(void);
	virtual ~PhysicsSystem(void);

	bool SphereSphereCollision(const CollisionSphere &s0, const CollisionSphere &s1, CollisionData &collisionData) const;
	bool AABBCollision(const CollisionAABB &cube0, const CollisionAABB &cube1) const;
	bool PointInConvexPolygon(const Vector3 testPosition, Vector3 * convexShapePoints, int numPointsL) const;

	static PhysicsSystem* instance;

	vector<PhysicsNode*> allNodes;
	static void AddCollisionImpulse(PhysicsNode& c0, PhysicsNode& c1, CollisionData cd, bool applyRot1 = true, bool applyRot2 = true);
	static void AddCollisionImpulseLinear(PhysicsNode& c0, PhysicsNode& c1, CollisionData cd, bool applyRot1 = true, bool applyRot2 = true);
	
	static float LengthSq(const Vector3 length);
	vector<PhysicsNode*> tempNodes;
	vector<PhysicsNode*> staticNodes;
};

